#include "ShearElementCalculator.h"

#include <assert.h>

#include <cmath>
#include <iostream>

#include "Utility/Algebra/Matrix.h"
#include "Utility/Algebra/Tools.h"
#include "Utility/Algebra/Vector3D.h"

#include "DataStructure/Common/Out.h"
#include "DataStructure/Input/Element/CSHEARData.h"
#include "DataStructure/Input/Property/PSHEARData.h"
#include "DataStructure/Result/ElementalStress/SHEARStressData.h"

#include "DBManager/IService/Input/Element/IElementService.h"
#include "DBManager/IService/Input/Material/IIsotropicMaterialService.h"
#include "DBManager/IService/Input/Node/INodeService.h"
#include "DBManager/IService/Input/Property/IPSHEARService.h"
#include "DBManager/IService/Result/ElementalForce/ISHEARForceService.h"
#include "DBManager/IService/Result/ElementalStress/ISHEARStressService.h"

#include "SGFem/Pre/Assembly/Assembly.h"
#include "SGFem/Pre/Assembly/CoordinateProcessor.h"
#include "SGFem/Pre/Assembly/ModelInfoQuery.h"

#include "../Common/ElementTools.hpp"

using SG::Algebra::Matrixd;
using SG::Algebra::Point;
using SG::Algebra::PointArray;
using SG::Algebra::Vector3D;
using SG::DataStructure::Common::Id;
using SG::DataStructure::Common::Real;
using SG::DataStructure::Common::Status;
using namespace SG::DataStructure::FEM;
using SG::DataStructure::Post::SHEARForceData;
using SG::DataStructure::Post::SHEARStressData;
using namespace SG::DBManager;
using SG::FEMSolver::CoordinateProcessor;
using namespace SG::FEMSolver::Element;

void ShearElementCalculator::Initialize (SG::DataStructure::FEM::ElementTypes type)
{
    m_type = type;
}

/** @brief 计算单元坐标转换矩阵
 *
 * 以 1-3， 2-4节点连线叉乘计算坐标系的Z轴（同时确定节点参考平面）
 * 1-2节点连线向法线确定的方向投影确定坐标系 x 轴
 */
void ShearElementCalculator::ComputeCoordTrans (const PointArray& points,
                                                Matrixd& _OUT     Tel,
                                                PointArray& _OUT  localPoints,
                                                Real& _OUT        area,
                                                Real& _OUT        warp) const
{
    // 检验是否是4节点单元
    assert (4 == points.size ());

    // 计算单元平面法线做为Z轴
    auto r13 = Distance (points[2], points[0]);
    auto r24 = Distance (points[3], points[1]);
    auto r41 = Distance (points[0], points[3]);
    auto n = SG::Algebra::Cross (r13, r24);
    Real norm;
    n.Normalize (norm);
    area = norm / 2.0;

    // 对1-2边投影，计算 x轴矢量
    auto r12  = Distance (points[1], points[0]);
    warp = SG::Algebra::dot (n, r12); //不共面翘曲
    r12       = r12 - n * warp;
    Real length12;
    r12.Normalize (length12);

    // 计算Y轴基矢量
    auto r2 = SG::Algebra::Cross (n, r12);

    Tel        = Matrixd (3, 3);
    Tel (0, 0) = r12.m_x;
    Tel (0, 1) = r12.m_y;
    Tel (0, 2) = r12.m_z;
    Tel (1, 0) = r2.m_x;
    Tel (1, 1) = r2.m_y;
    Tel (1, 2) = r2.m_z;
    Tel (2, 0) = n.m_x;
    Tel (2, 1) = n.m_y;
    Tel (2, 2) = n.m_z;

    // auto& origin = points[0];
    // Point::Transform (Tel, origin, points[1], _OUT localPoints[1]);
    // Point::Transform (Tel, origin, points[2], _OUT localPoints[2]);
    // Point::Transform (Tel, origin, points[3], _OUT localPoints[3]);


    // 节点1置为局部坐标系原点
    localPoints[0].m_x = 0.0;
    localPoints[0].m_y = 0.0;
    localPoints[0].m_z = 0.0; 
    // 节点2局部坐标
    localPoints[1].m_x = length12;
    localPoints[1].m_y = 0.0;
    localPoints[1].m_z = 0.0;

    // 节点3局部坐标
    localPoints[2].m_x = r12.m_x * r13.m_x + r12.m_y * r13.m_y + r12.m_z * r13.m_z;
    localPoints[2].m_y = r2.m_x * r13.m_x + r2.m_y * r13.m_y + r2.m_z * r13.m_z;
    localPoints[2].m_z = 0.0;

    // 节点4局部坐标
    localPoints[3].m_x = -(r12.m_x * r41.m_x + r12.m_y * r41.m_y + r12.m_z * r41.m_z);
    localPoints[3].m_y = -(r2.m_x * r41.m_x + r2.m_y * r41.m_y + r2.m_z * r41.m_z);
    localPoints[3].m_z = 0.0;
}

/** @brief 计算应变能
 *
 */
Real ShearElementCalculator::computeAdaptability (
    const PointArray& localPoints, const Real area, const Real G, const PSHEARData& property, const Real mu, Real* const pArray) const
{
    const Real t  = property.m_thickness;
    const Real x1 = 0.0, y1 = 0.0;
    const Real x2 = localPoints[1].m_x, y2 = 0.0;
    const Real x3 = localPoints[2].m_x, y3 = localPoints[2].m_y;
    const Real x4 = localPoints[3].m_x, y4 = localPoints[3].m_y;
    const Real tol = 0.01; //TODO: CHECK NASTRAN默认值是0.1，这里是0.01
    // const Real tol = 0.1;

    assert(y3 > 0.0);
    assert(x3 > (y3/y4 *x4));
    assert(y4 > 0.0);
    assert(x4 < (x2 - (x2-x3) * y4 / y3));

    Real& P1 = pArray[0];
    Real& P2 = pArray[1];
    Real& P3 = pArray[2];
    Real& P4 = pArray[3];

    // line34 是否平行 line12
    const bool flag1 = (fabs ((y3 - y4) / (x3 - x4)) < tol);
    // line13 与 line12 是否平行
    const bool flag2 = (fabs ((y4 * (x3 - x2) - y3 * x4)) < fabs (x4 * (x3 - x2) + y4 * y3) * tol);

    Real adaptability;
    if (flag1)
    {
        if (flag2)
        {
            // THE PANEL APPROXIMATES A PARALLELOGRAM.
            const Real D = -0.5 * (x4 / y4 + (x3 - x2) / y3 + (y3 - y4) / (x3 - x4));
            adaptability = area / (2 * G * t) * (1.0 + 2.0 * D * D / (1.0 + mu));
            P1           = 1.0;
            P2           = 1.0;
            P3           = 1.0;
            P4           = 1.0;
        }
        else
        {
            // THE LINE CONNECTING POINTS 3 AND 4 IS PARALLEL TO THE LINE CONNECTING POINTS 1 AND 2.
            const Real temp = y3 * x4 - y4 * (x3 - x2); // 计算行列式， x1,y1,y2 = 0
            const Real YP   = x2 * y3 * y4 / temp; // 利用克莱姆法则求两直线交点（y坐标）
            P1              = YP - y1;
            P2              = YP - y2;
            P3              = YP - y3;
            P4              = YP - y4; // 四个点到交点P的距离（y分量）
            const Real XP   = x2 * y3 * x4 / temp; // 两直线交点（x坐标）
            const Real SA   = (x2 - XP) / YP; // cot 角2？
            const Real C    = (x1 - XP) / YP; // cot 角1？
            adaptability    = (P1 * P2 * area) / (P3 * P4 * 2.0 * G * t) * (1.0 + 2.0 / (3.0 + 3.0 * mu) * (SA * SA + SA * C + C * C));
        }
    }
    else
    {
        if (flag2)
        {
            // THE LINE CONNECTING POINTS 1 AND 4 IS PARALLEL TO THE LINE CONNECTING POINTS 2 AND 3.
            const Real D    = -0.5 * (x4 / y4 + (x3 - x2) / y3); //平均斜率？
            const Real xQ   = x4 - y4 * (x3 - x4) / (y3 - y4); // 计算交点，Q的y坐标为0
            Real       temp = 1.0 / sqrt (1.0 + D * D);
            P1              = (xQ - x1 - D * y1) * temp;
            P2              = (xQ - x2 - D * y2) * temp;
            P3              = (xQ - x3 - D * y3) * temp;
            P4              = (xQ - x4 - D * y4) * temp;
            temp            = xQ - x4;
            const Real B    = (temp * D + y4) / (temp - y4 * D);
            adaptability    = ((P1 * P2 * area) / (P3 * P4 * 2.0 * G * t)) * (1.0 + 2.0 / (3.0 + 3.0 * mu) * (B * B + B * D + D * D));
        }
        else
        {
            // NO PARALLEL EFFECTS EXIST
            const Real XQ    = x4 - y4 * (x3 - x4) / (y3 - y4);
            Real       temp  = y3 * x4 - y4 * (x3 - x2);
            const Real XP    = x2 * y3 * x4 / temp;
            const Real YP    = x2 * y3 * y4 / temp;
            const Real XL    = sqrt ((XQ - XP) * (XQ - XP) + YP * YP); //PQ的距离
            const Real D     = (XQ - XP) / YP; //PQ斜率
            temp             = YP / XL;
            P1               = temp * (XQ - x1 - D * y1);
            P2               = temp * (XQ - x2 - D * y2);
            P3               = temp * (XQ - x3 - D * y3);
            P4               = temp * (XQ - x4 - D * y4); // 四个点到PQ的距离
            const Real C     = XL / P1 - D;
            const Real B     = XL / P4 - C;
            const Real A     = XL / P2 - D;
            const Real A2    = A * A;
            const Real B2    = B * B;
            const Real C2    = C * C;
            const Real D2    = D * D;
            const Real A3    = A2 * A;
            const Real B3    = B2 * B;
            const Real C3    = C2 * C;
            const Real D3    = D2 * D;
            const Real A4    = A3 * A;
            const Real B4    = B3 * B;
            const Real C4    = C3 * C;
            const Real D4    = D3 * D;
            const Real A5    = A4 * A;
            const Real B5    = B4 * B;
            const Real C5    = C4 * C;
            const Real D5    = D4 * D;
            temp             = 0.50 * P1 * P2 * P3 * P4 / (XL * XL);
            const Real TERM  = (A + B + 2.0 / 3.0 * (A3 + B3) + 0.2 * (A5 + B5)) * log (fabs (A + B));
            const Real TERM1 = (C + D + 2.0 / 3.0 * (C3 + D3) + 0.2 * (C5 + D5)) * log (fabs (C + D));
            const Real TERM2 = (B + C + 2.0 / 3.0 * (B3 + C3) + 0.2 * (B5 + C5)) * log (fabs (B + C));
            const Real TERM3 = (D + A + 2.0 / 3.0 * (D3 + A3) + 0.2 * (D5 + A5)) * log (fabs (D + A));
            const Real TERM4 = 0.1 * ((A2 - C2) * (B3 - D3) + (B2 - D2) * (A3 - C3));
            const Real TERM5 = 0.2 * ((A - C) * (B4 - D4) + (B - D) * (A4 - C4));
            const Real F     = temp * (TERM + TERM1 - TERM2 - TERM3 + TERM4 - TERM5);
            adaptability     = P1 * P2 / (P3 * P4 * 2.0 * G * t) * (area + 4.0 / (1.0 + mu) * (F - 2.0 / 3.0 * area));
        }
    }

    return adaptability;
}

/** @brief 计算增强矩阵
 *
 * nodeId1， nodeId2 为单元节点在母元中的编号， 可以获得一个位于剪切单元边的杆单元
 */
Matrixd ShearlinkCalc (const PointArray& localPoints, Real EA, std::size_t nodeId1, std::size_t nodeId2)
{
    Vector3D x = SG::Algebra::Distance (localPoints[nodeId1], localPoints[nodeId2]);
    Real     length;
    x.Normalize (length);

    const Real        c      = x.m_x;
    const Real        s      = x.m_y;
    const std::size_t posi1X = nodeId1 * 2;
    const std::size_t posi1Y = nodeId1 * 2 + 1;
    const std::size_t posi2X = nodeId2 * 2;
    const std::size_t posi2Y = nodeId2 * 2 + 1;

    Matrixd Ke (8, 8);
    Ke (posi1X, posi1X) = c * c;
    Ke (posi1X, posi1Y) = c * s;
    Ke (posi1X, posi2X) = -c * c;
    Ke (posi1X, posi2Y) = -c * s;
    Ke (posi1Y, posi1X) = c * s;
    Ke (posi1Y, posi1Y) = s * s;
    Ke (posi1Y, posi2X) = -c * s;
    Ke (posi1Y, posi2Y) = -s * s;
    Ke (posi2X, posi1X) = -c * c;
    Ke (posi2X, posi1Y) = -c * s;
    Ke (posi2X, posi2X) = c * c;
    Ke (posi2X, posi2Y) = c * s;
    Ke (posi2Y, posi1X) = -c * s;
    Ke (posi2Y, posi1Y) = -s * s;
    Ke (posi2Y, posi2X) = c * s;
    Ke (posi2Y, posi2Y) = s * s;
    Ke                  = Ke * (EA / length);

    return Ke;
}

/** @brief 计算单元刚度矩阵
 *
 */
void ShearElementCalculator::ComputeStiffness (
    const PointArray& localPoints, const Real Z, const Real area, const Real E, const PSHEARData& property, Matrixd& _OUT Kel) const
{
    const Real x1 = localPoints[0].m_x, y1 = localPoints[0].m_y;
    const Real x2 = localPoints[1].m_x, y2 = localPoints[1].m_y;
    const Real x3 = localPoints[2].m_x, y3 = localPoints[2].m_y;
    const Real x4 = localPoints[3].m_x, y4 = localPoints[3].m_y;
    const Real l13 = sqrt (x3 * x3 + y3 * y3); //对角线13的长度
    const Real l24 = sqrt ((x4 - x2) * (x4 - x2) + y4 * y4); //对角线24的长度
    Real       u1  = x3 / l13; //对角线13的单位向量分量1
    Real       u2  = (x4 - x2) / l24; //对角线24的单位向量分量1
    Real       v1  = y3 / l13; //对角线13的单位向量分量2
    Real       v2  = y4 / l24; //对角线24的单位向量分量2

    const Real temp  = x4 * y3 - x3 * y4;
    const auto AVEC1 = -0.5 * x2 * y4 * l13 / temp;
    const auto AVEC2 = 0.5 * x2 * y3 * l24 / (temp - x2 * (y3 - y4));
    const auto AVEC3 = -AVEC1;
    const auto AVEC4 = -AVEC2;
    Matrixd    F (8, 1);
    F (0, 0) = AVEC1 * u1;
    F (1, 0) = AVEC1 * v1;
    F (2, 0) = AVEC2 * u2;
    F (3, 0) = AVEC2 * v2;
    F (4, 0) = AVEC3 * u1;
    F (5, 0) = AVEC3 * v1;
    F (6, 0) = AVEC4 * u2;
    F (7, 0) = AVEC4 * v2;

    Kel = (F * F.Transpose ()) * (0.5 / Z);

    // 单元边增强效应
    const auto& t  = property.m_thickness;
    const auto& F1 = property.m_F[0];
    const auto& F2 = property.m_F[1];
    if (F1 > 0)
    {
        Real w1;
        if (F1 < 1.01)
        {
            const Real l12 = x2;
            const Real l34 = sqrt ((x4 - x3) * (x4 - x3) + (y4 - y3) * (y4 - y3));
            w1             = area * t / (l12 + l34) * F1;
        }
        else
        {
            w1 = 0.5 * F1 * t * t;
        }
        Kel = Kel + ShearlinkCalc (localPoints, E * w1, 0, 1);
        Kel = Kel + ShearlinkCalc (localPoints, E * w1, 2, 3);
    }
    if (F2 > 0)
    {
        Real w2;
        if (F2 < 1.01)
        {
            const Real l14 = sqrt ((x4 - x1) * (x4 - x1) + (y4 - y1) * (y4 - y1));
            const Real l23 = sqrt ((x2 - x3) * (x2 - x3) + (y2 - y3) * (y2 - y3));
            w2             = area * t / (l23 + l14) * F2;
        }
        else
        {
            w2 = 0.5 * F2 * t * t;
        }
        Kel = Kel + ShearlinkCalc (localPoints, E * w2, 0, 3);
        Kel = Kel + ShearlinkCalc (localPoints, E * w2, 1, 2);
    }
}

/** @brief 计算单元质量矩阵
 *
 */
void ShearElementCalculator::ComputeMass (const PointArray& localPoints,
                                          const Real&       rho,
                                          const PSHEARData& property,
                                          Matrixd& _OUT     Mass) const
{
    Vector3D normal;

    Real A1 = SG::Algebra::calcTriArea (localPoints[3], localPoints[0], localPoints[1], normal);
    Real A2 = SG::Algebra::calcTriArea (localPoints[0], localPoints[1], localPoints[2], normal);
    Real A3 = SG::Algebra::calcTriArea (localPoints[1], localPoints[2], localPoints[3], normal);
    Real A4 = SG::Algebra::calcTriArea (localPoints[2], localPoints[3], localPoints[0], normal);

    const Real& thickness = property.m_thickness;
    // mi 计算节点i的分配质量
    // 3.0 三角形的质量向三个节点分配
    // 2.0 两个四边形，每个节点的质量分配了两次
    const Real m1 = rho * thickness / 3.0 * (A1 + A2 + A4) / 2.0;
    const Real m2 = rho * thickness / 3.0 * (A1 + A2 + A3) / 2.0;
    const Real m3 = rho * thickness / 3.0 * (A2 + A3 + A4) / 2.0;
    const Real m4 = rho * thickness / 3.0 * (A3 + A4 + A1) / 2.0;

    Mass = Matrixd (12, 12);

    Mass (0, 0)   = m1;
    Mass (1, 1)   = m1;
    Mass (2, 2)   = m1;
    Mass (3, 3)   = m2;
    Mass (4, 4)   = m2;
    Mass (5, 5)   = m2;
    Mass (6, 6)   = m3;
    Mass (7, 7)   = m3;
    Mass (8, 8)   = m3;
    Mass (9, 9)   = m4;
    Mass (10, 10) = m4;
    Mass (11, 11) = m4;
}

/**
 * @brief   : 计算单元节点的全局索引
 * @param   : assembly         全局管理
 * @param   : nodeIndexes      单元节点编号的索引
 * @param[out]   : globalIndex 节点自由度对应的全局编号
 * @author  : wanghuan
 * @date    : 2024.10.30
 */
void computeGlobalDOFId (const SG::FEMSolver::AssemblyEigen& assembly,
                         const std::vector<std::size_t>&     nodeIndexes,
                         std::vector<int>& _OUT              globalIndex)
{
    static const DofSet DOFs{ DOFDirections::DOF_Ux, DOFDirections::DOF_Uy, DOFDirections::DOF_Uz };
    assembly.GetGlobalDOFId (nodeIndexes, DOFs, _OUT globalIndex);
}

void ShearElementCalculator::ComputeElementMatries (const SG::DBManager::DBServiceFactorySP dbServiceFactory)
{
    auto& assembly     = AssemblyEigen::GetInstance ();
    auto& coordHandler = CoordinateProcessor::GetInstance ();

    CSHEARData                                eleData;
    PSHEARData                                property;
    SG::DataStructure::FEM::IsotropicMaterial material;

    std::vector<std::size_t> nodeIndexes (4);
    std::vector<Id>          dispCoordIds (4);
    std::vector<Point>       points (4);
    const auto&              nodeRepository  = dbServiceFactory->get<INodeService> ();
    const auto&              shearRepository = dbServiceFactory->get<IElementService> ();
    const auto               N_NODE          = 4;
    std::vector<int>         globalIndex;

    Matrixd            Tel;
    std::vector<Point> localPoints (4);
    const auto         eleSize = shearRepository->GetElementSize (m_type);
    for (std::size_t iEle{ 0 }; iEle < eleSize; ++iEle)
    {
        /** 从数据库获取对应的单元数据 */
        shearRepository->FindByIndex (iEle, _OUT eleData);

        // Node coordinate
        ModelInfoQuery::QueryNode (nodeRepository, eleData.m_g, N_NODE, _OUT points, _OUT nodeIndexes, _OUT dispCoordIds);

        computeGlobalDOFId (assembly, nodeIndexes, _OUT globalIndex);

        Real area, warp;
        ComputeCoordTrans (points, Tel, localPoints, area, warp);

        auto status = dbServiceFactory->get<IPSHEARService> ()->FindById (eleData.m_pId, _OUT property);
        if (Status::STATUS_Failure == status)
        {
            std::cout << "can't find shear element prop \n";
        }
        dbServiceFactory->get<IIsotropicMaterialService> ()->FindById (property.m_mId, _OUT material);
        const Real& E  = material.m_E;
        const Real& mu = material.m_nu;
        const Real& G  = material.m_G;

        // ToDo 增加形状检查

        Real       pSet[4];
        const auto adaptability = computeAdaptability (localPoints, area, G, property, mu, pSet);

        Matrixd kLocal (8, 8);
        ComputeStiffness (localPoints, adaptability, area, E, property, _OUT kLocal);
        Matrixd T (8, 12);
        auto    Tel23 = Matrixd (2, 3);
        Tel23 (0, 0)  = Tel (0, 0);
        Tel23 (0, 1)  = Tel (0, 1);
        Tel23 (0, 2)  = Tel (0, 2);
        Tel23 (1, 0)  = Tel (1, 0);
        Tel23 (1, 1)  = Tel (1, 1);
        Tel23 (1, 2)  = Tel (1, 2);
        for (size_t i = 0; i < 4; ++i)
        {
            // 处理节点位移坐标系CD
            if (0 == dispCoordIds[i])
            {
                T.BlockFill (i * 2, i * 3, Tel23);
            }
            else
            {
                Matrixd TDisp (3, 3);
                coordHandler.GetCoordTrans (dispCoordIds[i], points[i], _OUT TDisp);
                T.BlockFill (i * 2, i * 3, Tel23 * TDisp.Transpose ());
            }
        }

        auto kGlobal = T.Transpose () * kLocal * T;
        assembly.AccumulateStiffness (globalIndex, kGlobal);

        // 组装质量矩阵
        Matrixd Mass (12, 12);
        ComputeMass (localPoints, material.m_rho, property, _OUT Mass);
        // 注意：Mass 在单元局部坐标系和位移输出坐标系的值相同，因此不需要进行坐标变换
        assembly.AccumulateCoupledMass (globalIndex, Mass);
    }
}

void ShearElementCalculator::ComputeElementForceAndStress (const PointArray&                         localPoints,
                                                           const SG::DataStructure::FEM::PSHEARData& property,
                                                           const IsotropicMaterial&                  material,
                                                           Real                                      area,
                                                           Real                                      warp,
                                                           Matrixd&                                  eleDisp,
                                                           Matrixd&                                  Tel23,
                                                           SHEARForceData& _OUT                      eleForceData,
                                                           SHEARStressData& _OUT                     eleStressData) const
{
    const Real& mu = material.m_nu;
    const Real& G  = material.m_G;
    const Real& t  = property.m_thickness;

    Real pSet[4];
    auto Z = computeAdaptability (localPoints, area, G, property, mu, pSet);

    const Real x2 = localPoints[1].m_x;
    const Real x3 = localPoints[2].m_x, y3 = localPoints[2].m_y;
    const Real x4 = localPoints[3].m_x, y4 = localPoints[3].m_y;
    const Real l13 = sqrt (x3 * x3 + y3 * y3);
    const Real l24 = sqrt ((x4 - x2) * (x4 - x2) + y4 * y4);
    Real       ASet[4];
    const Real temp = x4 * y3 - x3 * y4;
    ASet[0]         = -0.5 * x2 * y4 * l13 / temp;
    ASet[1]         = 0.5 * x2 * y3 * l24 / (temp - x2 * (y3 - y4));
    ASet[2]         = -ASet[0];
    ASet[3]         = -ASet[1];

    Real u[4]{ x3 / l13, (x4 - x2) / l24, x3 / l13, (x4 - x2) / l24 };
    Real v[4]{ y3 / l13, y4 / l24, y3 / l13, y4 / l24 };

    Real tauBar{ 0.0 };
    Real tauABS{ 0.0 };
    for (std::size_t iNode{ 0 }; iNode < 4; ++iNode)
    {
        // Si = - Ai/(4*Zt){ui : vi} [Te]T [Ti] * disp
        Matrixd uv (1, 2, { u[iNode], v[iNode] });
        auto    tmp = uv * Tel23;
        // 节点位移
        const auto posi  = iNode * 3;
        auto       dispI = Matrixd (3, 1, { eleDisp (posi, 0), eleDisp (posi + 1, 0), eleDisp (posi + 2, 0) });
        tmp              = tmp * dispI;

        auto s = -0.5 / (Z * t) * ASet[iNode] * tmp (0, 0);

        tauBar += s;
        tauABS += std::fabs (s);
    }

    Real tau[4];
    tau[0] = pSet[1] / pSet[0] * tauBar;
    tau[1] = pSet[0] / pSet[1] * tauBar;
    tau[2] = pSet[1] * pSet[0] / (pSet[2] * pSet[2]) * tauBar;
    tau[3] = pSet[1] * pSet[0] / (pSet[3] * pSet[3]) * tauBar;
    
    // Real ctu[4];
    // ctu[0] = std::fabs (pSet[1] / pSet[0]) * tauABS;
    // ctu[1] = std::fabs (pSet[0] / pSet[1]) * tauABS;
    // ctu[2] = std::fabs (pSet[1] * pSet[0] / (pSet[2] * pSet[2])) * tauABS;
    // ctu[3] = std::fabs (pSet[1] * pSet[0] / (pSet[3] * pSet[3])) * tauABS;
    eleStressData.m_TAVG = 0.25 * (tau[0] + tau[1] + tau[2] +tau[3]);
    eleStressData.m_TMAX = std::fabs(tau[0]);
    for (size_t itau = 1; itau < 4; itau++)
    {
        const auto val = std::fabs(tau[itau]);
        if (val > eleStressData.m_TMAX){eleStressData.m_TMAX = val;}
    }

    //margin of safety //TODO:缺少材料的剪切容许应力
    if (std::fabs(eleStressData.m_TMAX) < 1e-9 ){eleStressData.m_MS = 1.0;}
    
    
    Real RQ[4];
    RQ[0] = pSet[0] / pSet[2] * t;
    RQ[1] = pSet[1] * pSet[0] / (pSet[2] * pSet[3]) * t;
    RQ[2] = pSet[1] / pSet[3] * t;
    RQ[3] = - 0.5 * warp;
    
    const Real l12 = sqrt (x2 * x2);
    const Real l23 = sqrt ((x3 - x2) * (x3 - x2) + y3 * y3);
    const Real l34 = sqrt ((x4 - x3) * (x4 - x3) + (y4 - y3) * (y4 - y3));
    const Real l14 = sqrt (x4 * x4 + y4 * y4);

    Real RK[4];
    RK[0] = 0.5 * l12;
    RK[1] = 0.5 * l23;
    RK[2] = 0.5 * l34;
    RK[3] = 0.5 * l14;

    const Real RQ1 = tauBar * t / sqrt (1 + (RQ[3] / RK[0]) * (RQ[3] / RK[0]));
    const Real RQ2 = tauBar * RQ[0] / sqrt (1 + (RQ[3] / RK[1]) * (RQ[3] / RK[1]));
    const Real RQ3 = tauBar * RQ[1] / sqrt (1 + (RQ[3] / RK[2]) * (RQ[3] / RK[2]));
    const Real RQ4 = tauBar * RQ[2] / sqrt (1 + (RQ[3] / RK[3]) * (RQ[3] / RK[3]));

    Real RK1 = -(RQ1 + RQ4) * RQ[3];
    Real RK2 = -(RQ1 + RQ2) * RQ[3];
    Real RK3 = -(RQ2 + RQ3) * RQ[3];
    Real RK4 = -(RQ3 + RQ4) * RQ[3];

    eleForceData.m_F41 = RQ4 * RK[3];
    eleForceData.m_F21 = RQ1 * RK[0];
    eleForceData.m_F12 = -1.0 * RQ1 * RK[0];
    eleForceData.m_F32 = -1.0 * RQ2 * RK[1];
    eleForceData.m_F23 = RQ2 * RK[1];
    eleForceData.m_F43 = RQ3 * RK[2];
    eleForceData.m_F34 = -1.0 * RQ3 * RK[2];
    eleForceData.m_F14 = -1.0 * RQ4 * RK[3];
    eleForceData.m_KF1 = RK1;
    eleForceData.m_S12 = RQ1;
    eleForceData.m_KF2 = RK2;
    eleForceData.m_S23 = RQ2;
    eleForceData.m_KF3 = RK3; 
    eleForceData.m_S34 = RQ3;
    eleForceData.m_KF4 = RK4;
    eleForceData.m_S41 = RQ4;
}

// 后处理数据
void ShearElementCalculator::DataRecovery (const SG::DBManager::DBServiceFactorySP             dbServiceFactory,
                                           int                                                 subcaseId, 
                                           SG::DataStructure::Common::Real                     time,
                                           const std::vector<SG::DataStructure::Common::Real>& globalDisplacement)
{
    auto& assembly     = AssemblyEigen::GetInstance ();

    std::vector<std::size_t> nodeIndexes (4);
    std::vector<Id>          dispCoordIds (4);
    std::vector<Point>       points (4);
    const auto&              nodeRepository  = dbServiceFactory->get<INodeService> ();
    const auto&              shearRepository = dbServiceFactory->get<IElementService> ();
    const auto               N_NODE          = 4;
    std::vector<int>         dofGlobalIds;

    Matrixd eleDisp (12, 1);

    CSHEARData                                eleData;
    PSHEARData                                property;
    SHEARForceData                            eleForceData;
    SHEARStressData                           eleStressData;
    SG::DataStructure::FEM::IsotropicMaterial material;

    Matrixd            Tel;
    std::vector<Point> localPoints (4);

    const auto                  eleSize = shearRepository->GetElementSize (m_type);
    std::vector<SHEARForceData> eleForceDataSet (eleSize);
    std::vector<SHEARStressData> eleStressDataSet (eleSize);
    for (std::size_t iEle{ 0 }; iEle < eleSize; ++iEle)
    {
        /** 从数据库获取对应的单元数据 */
        shearRepository->FindByIndex (iEle, _OUT eleData);
        ModelInfoQuery::QueryNode (nodeRepository, eleData.m_g, N_NODE, _OUT points, _OUT nodeIndexes, _OUT dispCoordIds);

        // 查询单元在全局坐标，全局坐标下的单元节点位移
        computeGlobalDOFId (assembly, nodeIndexes, _OUT dofGlobalIds);
        ElementTools::GetElementDisplacement (dofGlobalIds, globalDisplacement, _OUT eleDisp);

        auto status = dbServiceFactory->get<IPSHEARService> ()->FindById (eleData.m_pId, _OUT property);
        if (Status::STATUS_Failure == status)
        {
            std::cout << "can't find shear element prop \n";
        }
        dbServiceFactory->get<IIsotropicMaterialService> ()->FindById (property.m_mId, _OUT material);

        Real               area;
        Real               warp;
        std::vector<Point> localPoints (4);
        ComputeCoordTrans (points, Tel, localPoints, area, warp);

        auto Tel23   = Matrixd (2, 3);
        Tel23 (0, 0) = Tel (0, 0);
        Tel23 (0, 1) = Tel (0, 1);
        Tel23 (0, 2) = Tel (0, 2);
        Tel23 (1, 0) = Tel (1, 0);
        Tel23 (1, 1) = Tel (1, 1);
        Tel23 (1, 2) = Tel (1, 2);
        ComputeElementForceAndStress (localPoints, property, material, area, warp, eleDisp, Tel23, _OUT eleForceData, _OUT eleStressData);
        eleForceData.m_eId  = eleData.m_id;
        eleStressData.m_eId = eleData.m_id;
        eleForceDataSet[iEle] = eleForceData;
        eleStressDataSet[iEle] = eleStressData;
    }

    // write to database
    dbServiceFactory->get<ISHEARForceService> ()->AddBySubcaseId (static_cast<Id> (subcaseId), eleForceDataSet);
    dbServiceFactory->get<ISHEARStressService> ()->AddBySubcaseId (static_cast<Id> (subcaseId), eleStressDataSet);
}

// 计算全局坐标系下单元应力刚度矩阵
void ShearElementCalculator::ComputeStressStiffness (const SG::DBManager::DBServiceFactorySP dbServiceFactory, int staticSubId)
{}